#ifndef CAROS_KUKA_LWR_MSGS_H
#define CAROS_KUKA_LWR_MSGS_H

#include <geometry_msgs/WrenchStamped.h>
#include <ros/ros.h>
#include <std_msgs/String.h>
#include <unistd.h>
#include <boost/asio.hpp>
#include <boost/bind.hpp>
#include <cstdlib>
#include <deque>
#include <fstream>
#include <iostream>
#include <rw/math.hpp>
#include <string>
#include <thread>  // NOLINT(build/c++11)

// inputs:
// bye
// hello
// smart joint move  q,j_speed
// direct joint move  q,j_speed
// smart cartesian move  tool_frame,speed
// direct cartesian move  tool_frame,speed
// joint move  q,j_speed, blend
// lin move  tool_frame,speed,blend
// ptp move  tool_frame, speed, blend
// linr move  tool_frame,speed, blend, redundancy
// ptpr move  tool_frame, speed, blending, redundancy
// ptpforcez move  tool_frame, speed, blending, force thresholdZ
// linforcez move  tool_frame, speed, blending, force threshold Z
// ptpforce move  tool_frame, speed, blending, force threshold XYZ
// linforce move  tool_frame, speed, blending, force threshold XYZ
// ptpstiff move  tool_frame, speed, f_xyz, t_xyz
// linstiff move  tool_frame, speed, stiffness, f_xyz, t_xyz
// ptpcompliant move  tool_frame, speed, blending, f_xyz, t_xyz
// lincompliant move  tool_frame, speed, blending, f_xyz, t_xyz
// start gravity  stiffness
// stop
// feedback
// frame
// Tool
// Flange
// joint_position
// status
// cartesian_force
// joint_torque
// get tool frame
// get flange frame
// get status
// get cartesian force
// get joint position
// get joint torque

namespace caros
{
std::string to_string(rw::math::Q q);
std::string to_string(rw::math::Transform3D<double> T);
std::string to_string(geometry_msgs::WrenchStamped w);

// std::string smart_move_q(rw::math::Q target, rw::math::Q velocity = rw::math::Q(7,1.0,1.0,1.0,1.0,1.0,1.0,1.0););
// std::string direct_move_q(rw::math::Q target, rw::math::Q velocity = rw::math::Q(7,1.0,1.0,1.0,1.0,1.0,1.0,1.0););
// std::string smart_move(rw::math::Transform3D<double> target, double velocity=1.0);
// std::string direct_move(rw::math::Transform3D<double> target, double velocity=1.0);
std::string move_q(rw::math::Q target, rw::math::Q velocity, double blend);
std::string move_q(rw::math::Q target, rw::math::Q velocity);
std::string move_q(rw::math::Q target);
std::string lin_move(rw::math::Transform3D<double> target, double velocity = 1.0, double blend = 20.0);
std::string ptp_move(rw::math::Transform3D<double> target, double velocity = 1.0, double blend = 20.0);
std::string ptp_move_path(rw::math::Q target, double velocity = 1.0, double blend = 20.0, bool first = false,
                          bool last = false);
std::string linr_move(rw::math::Transform3D<double> target, double velocity = 1.0, double blend = 20.0,
                      double redundancy = M_PI);
std::string ptpr_move(rw::math::Transform3D<double> target, double velocity = 1.0, double blend = 20.0,
                      double redundancy = M_PI);
std::string linforcex_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity = 1.0,
                           double blend = 20.0);
std::string linforcey_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity = 1.0,
                           double blend = 20.0);
std::string linforcez_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity = 1.0,
                           double blend = 20.0);
std::string forcex_move(rw::math::Transform3D<double> target, double force_low, double force_high,
                        double velocity = 1.0, double blend = 20.0);
std::string forcey_move(rw::math::Transform3D<double> target, double force_low, double force_high,
                        double velocity = 1.0, double blend = 20.0);
std::string forcez_move(rw::math::Transform3D<double> target, double force_low, double force_high,
                        double velocity = 1.0, double blend = 20.0);
std::string ptpforcez_move(rw::math::Transform3D<double> target, double force_threshold_z, double velocity = 1.0,
                           double blend = 20.0);
std::string linforce_move(rw::math::Transform3D<double> target, double force_threshold_x, double force_threshold_y,
                          double force_threshold_z, double velocity = 1.0, double blend = 20.0);
std::string ptpforce_move(rw::math::Transform3D<double> target, double force_threshold_x, double force_threshold_y,
                          double force_threshold_z, double velocity = 1.0, double blend = 20.0);
std::string ptpstiff_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                          double velocity = 1.0);
std::string linstiff_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                          double stiffness, double velocity = 1.0);
std::string ptpcompliant_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                              double velocity = 1.0, double blend = 20.0);
std::string lincompliant_move(rw::math::Transform3D<double> target, geometry_msgs::WrenchStamped impedance,
                              double stiffness, double velocity = 1.0, double blend = 20.0);
std::string qcompliant_move(rw::math::Q target, geometry_msgs::WrenchStamped impedance, double velocity = 1.0,
                            double blend = 20.0);
std::string start_gravity(double stiffness);
std::string stop();
std::string update();
std::string exit();
std::string set_velocity(double speed = 0.5);

}  // namespace caros

#endif  // CAROS_KUKA_LWR_MSGS_H
